﻿#include "JoyStick2D.h"
#include <Windows.h>
#include <QDebug>



JoyStick2D::~JoyStick2D()
{
    stop();
}

bool JoyStick2D::listen()
{
    if(m_valid)
    {
        m_run = true;
        start();
        return true;
    }
    return false;
}

void JoyStick2D::stop()
{
    if(isRunning())
    {
        m_run = false;
        wait();
    }
}

JoyStick2D::JoyStick2D(QObject *parent)
    :QThread(parent),
      m_valid(false),
      pov_forward(false),
      pov_backward(false),
      pov_left(false),
      pov_right(false),
      pov_center(false),
      old_xPos(32767),
      old_yPos(32767),
      old_pov(JOY_POVCENTERED),
      m_run(false)

{
    JOYINFO joyinfo;
    if( joyGetNumDevs() > 0 && joyGetPos(JOYSTICKID1, &joyinfo) != JOYERR_UNPLUGGED )
    {
        qDebug() << "JoyStick Init success!";
        m_valid = true;
    }
    else
    {
        qDebug() << "JoyStick not found!";
        m_valid = false;
        m_run = false;
    }
    for(int i = 0; i < 16; i++)
    {
        m_button[i] = 0;
    }
}

void JoyStick2D::run()
{
    //JOYINFO joyinfo;
    JOYINFOEX joyinfoex;
    joyinfoex.dwSize = sizeof(JOYINFOEX);
    joyinfoex.dwFlags = JOY_RETURNALL;
    while(m_run)
    {
//        if(joyGetPos(JOYSTICKID1, &joyinfo) == JOYERR_NOERROR)
//        {
//            //qDebug() << joyinfo.wXpos;
//            AxisX_StateMachine(joyinfo.wXpos);
//            AxisY_StateMachine(joyinfo.wYpos);
//            Button_StateMachine(joyinfo.wButtons);
//        }
        if(joyGetPosEx(JOYSTICKID1, &joyinfoex) == JOYERR_NOERROR)
        {
            AxisX_StateMachine(joyinfoex.dwXpos);
            AxisY_StateMachine(joyinfoex.dwYpos);
            Button_StateMachine(joyinfoex.dwButtons);
            POV_StateMachine_Axis(joyinfoex.dwPOV);
        }
        msleep(50);
    }
}

void JoyStick2D::POV_StateMachine(int pov)
{
    if(pov != old_pov)
    {
        //qDebug() << pov;
        if( pov == JOY_POVFORWARD )
        {
            emit Joy_POVForward();
        }
        if( pov == JOY_POVFORWARD + 4500 )
        {
            emit Joy_POVForwardRight();
        }
        if( pov == JOY_POVRIGHT )
        {
            emit Joy_POVRight();
        }
        if( pov == JOY_POVRIGHT  + 4500 )
        {
            emit Joy_POVBackwardRight();
        }
        if( pov == JOY_POVBACKWARD )
        {
            emit Joy_POVBackward();
        }
        if( pov == JOY_POVBACKWARD + 4500 )
        {
            emit Joy_POVBackwardLeft();
        }
        if( pov == JOY_POVLEFT )
        {
            emit Joy_POVLeft();
        }
        if( pov == JOY_POVLEFT + 4500 )
        {
            emit Joy_POVForwardLeft();
        }
        if( pov == JOY_POVCENTERED )
        {
            emit Joy_POVCentered();
        }
        old_pov = pov;
    }
}

void JoyStick2D::POV_StateMachine_Axis(int pov)
{
    if(pov != old_pov)
    {
        //qDebug() << pov;
        if( pov == JOY_POVFORWARD )
        {
            if(pov_forward == false)
            {
                pov_forward = true;
                emit Joy_MoveForward();
                //qDebug() << "JOY_POVFORWARD";
            }
            if(pov_left == true)
            {
                emit Joy_MoveLeftStop();
                //qDebug() << "JOY_POVLEFT RETURN";
            }
            if(pov_right == true)
            {
                emit Joy_MoveRightStop();
                //qDebug() << "JOY_POVRIGHT RETURN";
            }
            pov_left = false;
            pov_right = false;
        }
        if( pov == JOY_POVFORWARD + 4500 )
        {
            if(pov_forward == false)
            {
                pov_forward = true;
                emit Joy_MoveForward();
                //qDebug() << "JOY_POVFORWARD";
            }
            if(pov_right == false)
            {
                pov_right = true;
                emit Joy_MoveRight();
                //qDebug() << "JOY_POVRIGHT";
            }
        }
        if( pov == JOY_POVRIGHT )
        {
            if(pov_right == false)
            {
                pov_right = true;
                emit Joy_MoveRight();
                //qDebug() << "JOY_POVRIGHT";
            }
            if(pov_forward == true)
            {
                emit Joy_MoveForwardStop();
                //qDebug() << "JOY_POVFORWARD RETURN";
            }
            if(pov_backward == true)
            {
                emit Joy_MoveBackwardStop();
                //qDebug() << "JOY_POVBACKWARD RETURN";
            }
            pov_forward = false;
            pov_backward = false;
        }
        if( pov == JOY_POVRIGHT  + 4500 )
        {
            if(pov_right == false)
            {
                pov_right = true;
                emit Joy_MoveRight();
               // qDebug() << "JOY_POVRIGHT";
            }
            if(pov_backward == false)
            {
                pov_backward = true;
                emit Joy_MoveBackward();
               // qDebug() << "JOY_POVBACKWARD";
            }
        }
        if( pov == JOY_POVBACKWARD )
        {
            if(pov_backward == false)
            {
                pov_backward = true;
                emit Joy_MoveBackward();
                //qDebug() << "JOY_POVBACKWARD";
            }
            if(pov_left == true)
            {
                emit Joy_MoveLeftStop();
                //qDebug() << "JOY_POVLEFT RETURN";
            }
            if(pov_right == true)
            {
                emit Joy_MoveRightStop();
                //qDebug() << "JOY_POVRIGHT RETURN";
            }
            pov_left = false;
            pov_right = false;
        }
        if( pov == JOY_POVBACKWARD + 4500 )
        {
            if(pov_backward == false)
            {
                pov_backward = true;
                emit Joy_MoveBackward();
                //qDebug() << "JOY_POVBACKWARD";
            }
            if(pov_left == false)
            {
                pov_left = true;
                emit Joy_MoveLeft();
               // qDebug() << "JOY_POVLEFT";
            }
        }
        if( pov == JOY_POVLEFT )
        {
            if(pov_left == false)
            {
                pov_left = true;
                emit Joy_MoveLeft();
                //qDebug() << "JOY_POVLEFT";
            }
            if(pov_forward == true)
            {
                emit Joy_MoveForwardStop();
                //qDebug() << "JOY_POVFORWARD RETURN";
            }
            if(pov_backward == true)
            {
                emit Joy_MoveBackwardStop();
                //qDebug() << "JOY_POVBACKWARD RETURN";
            }
            pov_backward = false;
            pov_forward = false;
        }
        if( pov == JOY_POVLEFT + 4500 )
        {
            if(pov_left == false)
            {
                pov_left = true;
                emit Joy_MoveLeft();
                //qDebug() << "JOY_POVLEFT";
            }
            if(pov_forward == false)
            {
                pov_forward = true;
                emit Joy_MoveForward();
                //qDebug() << "JOY_POVFORWARD";
            }
        }
        if( pov == JOY_POVCENTERED )
        {
            if(pov_forward == true)
            {
                emit Joy_MoveForwardStop();
                //qDebug() << "JOY_POVFORWARD RETURN";
            }
            if(pov_backward == true)
            {
                emit Joy_MoveBackwardStop();
                //qDebug() << "JOY_POVBACKWARD RETURN";
            }
            if(pov_left == true)
            {
                emit Joy_MoveLeftStop();
                //qDebug() << "JOY_POVLEFT RETURN";
            }
            if(pov_right == true)
            {
                emit Joy_MoveRightStop();
                //qDebug() << "JOY_POVRIGHT RETURN";
            }
            pov_forward = false;
            pov_right = false;
            pov_left = false;
            pov_backward = false;
            //qDebug() << "JOY_POVCENTERED";
        }
        old_pov = pov;
    }
}

void JoyStick2D::Button_StateMachine(int button)
{
    const int mask[16] = {1, 2, 4, 8, 16, 32, 64, 128, 256, 512, 1024, 2048, 4096, 8192, 16384, 32768};
    for(int i = 0; i < 12; i++)
    {
        int n = button & mask[i];
        if(n ^ m_button[i])
        {
            m_button[i] = n;
            if( n )
            {
                switch (i + 1)
                {
                case 1:
                    emit Joy_Button1Pressed();
                    break;
                case 2:
                    emit Joy_Button2Pressed();
                    break;
                case 3:
                    emit Joy_Button3Pressed();
                    break;
                case 4:
                    emit Joy_Button4Pressed();
                    break;
                case 5:
                    emit Joy_Button5Pressed();
                    break;
                case 6:
                    emit Joy_Button6Pressed();
                    break;
                case 7:
                    emit Joy_Button7Pressed();
                    break;
                case 8:
                    emit Joy_Button8Pressed();
                    break;
                default:
                    break;
                }

                qDebug() << "Joy_ButtonPressed(" << i + 1 << ")";;
            }
            else
            {
                switch (i + 1)
                {
                case 1:
                    emit Joy_Button1Released();
                    break;
                case 2:
                    emit Joy_Button2Released();
                    break;
                case 3:
                    emit Joy_Button3Released();
                    break;
                case 4:
                    emit Joy_Button4Released();
                    break;
                case 5:
                    emit Joy_Button5Released();
                    break;
                case 6:
                    emit Joy_Button6Released();
                    break;
                case 7:
                    emit Joy_Button7Released();
                    break;
                case 8:
                    emit Joy_Button8Released();
                    break;
                default:
                    break;
                }
                qDebug() << "Joy_ButtonReleased(" << i + 1 << ")";
            }
        }
    }
}

void JoyStick2D::AxisX_StateMachine(int xPos)
{
    if(xPos == old_xPos) return;
    switch(xPos)
    {
    case 0:
        emit Joy_MoveLeft();
        break;

    case 65535:
        emit Joy_MoveRight();
        break;
    default:
        if(old_xPos == 0)
        {
            emit Joy_MoveLeftStop();
        }
        else
        {
            emit Joy_MoveRightStop();
        }
        break;
    }
    old_xPos = xPos;
}

void JoyStick2D::AxisY_StateMachine(int yPos)
{
    if(yPos == old_yPos) return;
    switch(yPos)
    {
    case 65535:
        emit Joy_MoveForward();
        break;
    case 0:
        emit Joy_MoveBackward();
        break;
    default:
        if(old_xPos == 0)
        {
            emit Joy_MoveForwardStop();
        }
        else
        {
            emit Joy_MoveBackwardStop();
        }
        break;
    }
    old_yPos = yPos;
}
